﻿using System;
using System.Collections.Generic;

namespace Raven
{
    public class RavenSteering
    {
        public enum summing_method
        {
            weighted_average,
            prioritized,
            dithered
        };

        [Flags]
        private enum BehaviorType
        {
            None = 0,
            Seek = 1 << 0,
            Arrive = 1 << 1,
            Wander = 1 << 2,
            WallAvoidance = 1 << 3,
            Separation = 1 << 4,
        }

        private enum Deceleration
        {
            slow = 3,
            normal = 2,
            fast = 1
        };

        //--------------------------- Constants ----------------------------------

        //the radius of the constraining circle for the wander behavior
        private Fix WanderRad = Fix.Ratio(12, 10);

        //distance the wander circle is projected in front of the agent
        Fix WanderDist = 2;

        //the maximum amount of displacement along the circle each frame
        Fix WanderJitterPerSec = 40;

        private RavenBot _bot;
        private RavenGame _world;
        private Vector2 _steeringForce;
        private RavenBot _targetAgent1;
        private RavenBot _targetAgent2;

        private Vector2 _target;

        private Vector2[] _feelers;

        private Fix _wallDetectionFeelerLength;

        private Vector2 _wanderTarget;

        //explained above
        private Fix m_dWanderJitter;
        private Fix m_dWanderRadius;
        private Fix m_dWanderDistance;

        //multipliers. These can be adjusted to effect strength of the  
        //appropriate behavior.
        private Fix m_dWeightSeparation;
        private Fix m_dWeightWander;
        private Fix m_dWeightWallAvoidance;
        private Fix m_dWeightSeek;
        private Fix m_dWeightArrive;

        //how far the agent can 'see'
        private Fix m_dViewDistance;

        //binary flags to indicate whether or not a behavior should be active
        private BehaviorType m_iFlags;


        //Arrive makes use of these to determine how quickly a Raven_Bot
        //should decelerate to its target

        //default
        private Deceleration m_Deceleration;

        //is cell space partitioning to be used or not?
        private bool m_bCellSpaceOn;

        //what type of method is used to sum any active behavior
        private summing_method m_SummingMethod;

        public void SeekOn()
        {
            m_iFlags |= BehaviorType.Seek;
        }

        public void ArriveOn()
        {
            m_iFlags |= BehaviorType.Arrive;
        }

        public void WanderOn()
        {
            m_iFlags |= BehaviorType.Wander;
        }

        public void WanderOff()
        {
            if (On(BehaviorType.Wander)) m_iFlags ^= BehaviorType.Wander;
        }

        public void SeparationOn()
        {
            m_iFlags |= BehaviorType.Separation;
        }

        public void WallAvoidanceOn()
        {
            m_iFlags |= BehaviorType.WallAvoidance;
        }

        public void SetTarget(Vector2 target)
        {
            _target = target;
        }


        public void SeekOff()
        {
            if (On(BehaviorType.Seek)) m_iFlags ^= BehaviorType.Seek;
        }

        public void ArriveOff()
        {
            if (On(BehaviorType.Arrive)) m_iFlags ^= BehaviorType.Arrive;
        }

        public void SeparationOff()
        {
            if (On(BehaviorType.Separation)) m_iFlags ^= BehaviorType.Separation;
        }

        public void WallAvoidanceOff()
        {
            if (On(BehaviorType.WallAvoidance)) m_iFlags ^= BehaviorType.WallAvoidance;
        }

        public bool SeekIsOn()
        {
            return On(BehaviorType.Seek);
        }

        public bool ArriveIsOn()
        {
            return On(BehaviorType.Arrive);
        }

        public bool WanderIsOn()
        {
            return On(BehaviorType.Wander);
        }

        public bool SeparationIsOn()
        {
            return On(BehaviorType.Separation);
        }

        public bool WallAvoidanceIsOn()
        {
            return On(BehaviorType.WallAvoidance);
        }


        public RavenSteering(RavenGame world, RavenBot bot)
        {
            _world = world;
            _bot = bot;
            m_iFlags = 0;
            m_dWeightSeparation = Constants.SeparationWeight;
            m_dWeightWander = Constants.WanderWeight;
            m_dWeightWallAvoidance = Constants.WallAvoidanceWeight;
            m_dViewDistance = Constants.ViewDistance;
            _wallDetectionFeelerLength = Constants.WallDetectionFeelerLength;
            _feelers = new Vector2[3];
            m_Deceleration = Deceleration.normal;
            _targetAgent1 = null;
            _targetAgent2 = null;
            m_dWanderDistance = WanderDist;
            m_dWanderJitter = WanderJitterPerSec;
            m_dWanderRadius = WanderRad;
            m_dWeightSeek = Constants.SeekWeight;
            m_dWeightArrive = Constants.ArriveWeight;
            m_bCellSpaceOn = false;
            m_SummingMethod = summing_method.prioritized;

            Fix theta = _world.GetRandom().RandFix() * FixMath.PI * 2;

            //create a vector to a target position on the wander circle
            _wanderTarget = new Vector2(m_dWanderRadius * FixMath.Cos(theta),
                m_dWanderRadius * FixMath.Sin(theta));
        }

        public Vector2 Force()
        {
            return _steeringForce;
        }

        public Vector2 Calculate()
        {
            //reset the steering force
            _steeringForce = Vector2.zero;

            //tag neighbors if any of the following 3 group behaviors are switched on
            if (On(BehaviorType.Separation))
            {
                _world.TagRavenBotsWithinViewRange(_bot, m_dViewDistance);
            }

            _steeringForce = CalculatePrioritized();

            return _steeringForce;
        }

        public Vector2 CalculatePrioritized()
        {
            Vector2 force;

            if (On(BehaviorType.WallAvoidance))
            {
                force = WallAvoidance(_world.GetMap().GetWalls()) *
                        m_dWeightWallAvoidance;

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }


            //these next three can be combined for flocking behavior (wander is
            //also a good behavior to add into this mix)

            if (On(BehaviorType.Separation))
            {
                force = Separation(_world.GetAllBots()) * m_dWeightSeparation;

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }


            if (On(BehaviorType.Seek))
            {
                force = Seek(_target) * m_dWeightSeek;

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }


            if (On(BehaviorType.Arrive))
            {
                force = Arrive(_target, m_Deceleration) * m_dWeightArrive;

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }

            if (On(BehaviorType.Wander))
            {
                force = Wander() * m_dWeightWander;

                if (!AccumulateForce(ref _steeringForce, force)) return _steeringForce;
            }


            return _steeringForce;
        }

        private bool On(BehaviorType bt)
        {
            return (m_iFlags & bt) > 0;
        }

        //--------------------- AccumulateForce ----------------------------------
        //
        //  This function calculates how much of its max steering force the 
        //  vehicle has left to apply and then applies that amount of the
        //  force to add.
        //------------------------------------------------------------------------
        private bool AccumulateForce(ref Vector2 RunningTot,
            Vector2 ForceToAdd)
        {
            //calculate how much steering force the vehicle has used so far
            Fix MagnitudeSoFar = RunningTot.magnitude;

            //calculate how much steering force remains to be used by this vehicle
            Fix MagnitudeRemaining = _bot.MaxForce() - MagnitudeSoFar;

            //return false if there is no more force left to use
            if (MagnitudeRemaining <= 0) return false;

            //calculate the magnitude of the force we want to add
            Fix MagnitudeToAdd = ForceToAdd.magnitude;

            //if the magnitude of the sum of ForceToAdd and the running total
            //does not exceed the maximum force available to this vehicle, just
            //add together. Otherwise add as much of the ForceToAdd vector is
            //possible without going over the max.
            if (MagnitudeToAdd < MagnitudeRemaining)
            {
                RunningTot += ForceToAdd;
            }

            else
            {
                MagnitudeToAdd = MagnitudeRemaining;

                //add it to the steering force
                RunningTot += (ForceToAdd.normalized * MagnitudeToAdd);
            }

            return true;
        }

        //--------------------------- Arrive -------------------------------------
        //
        //  This behavior is similar to seek but it attempts to arrive at the
        //  target with a zero velocity
        //------------------------------------------------------------------------
        Vector2 Arrive(Vector2 target,
            Deceleration deceleration)
        {
            Vector2 ToTarget = target - _bot.Pos();

            //calculate the distance to the target
            Fix dist = ToTarget.magnitude;

            if (dist > 0)
            {
                //because Deceleration is enumerated as an int, this value is required
                //to provide fine tweaking of the deceleration..
                Fix DecelerationTweaker = Fix.Ratio(3, 10);

                //calculate the speed required to reach the target given the desired
                //deceleration
                Fix speed = dist / ((Fix) (int) deceleration * DecelerationTweaker);

                //make sure the velocity does not exceed the max
                speed = FixMath.Min(speed, _bot.MaxSpeed());

                //from here proceed just like Seek except we don't need to normalize 
                //the ToTarget vector because we have already gone to the trouble
                //of calculating its length: dist. 
                Vector2 DesiredVelocity = ToTarget * speed / dist;

                return (DesiredVelocity - _bot.Velocity());
            }

            return Vector2.zero;
        }

        //------------------------------- Seek -----------------------------------
        //
        //  Given a target, this behavior returns a steering force which will
        //  direct the agent towards the target
        //------------------------------------------------------------------------
        Vector2 Seek(Vector2 target)
        {
            Vector2 DesiredVelocity = (target - _bot.Pos()).normalized
                                      * _bot.MaxSpeed();

            return (DesiredVelocity - _bot.Velocity());
        }

        //--------------------------- Wander -------------------------------------
        //
        //  This behavior makes the agent wander about randomly
        //------------------------------------------------------------------------
        Vector2 Wander()
        {
            //first, add a small random vector to the target's position
            _wanderTarget += new Vector2(_world.GetRandom().RandomClamped() * m_dWanderJitter,
                _world.GetRandom().RandomClamped() * m_dWanderJitter);

            //reproject this new vector back on to a unit circle
            _wanderTarget.Normalize();

            //increase the length of the vector to the same as the radius
            //of the wander circle
            _wanderTarget *= m_dWanderRadius;

            //move the target into a position WanderDist in front of the agent
            Vector2 target = _wanderTarget + new Vector2(m_dWanderDistance, 0);

            //project the target into world space
            Vector2 Target = Transformations.PointToWorldSpace(target,
                _bot.Heading(),
                _bot.Side(),
                _bot.Pos());

            //and steer towards it
            return Target - _bot.Pos();
        }

        //------------------------------- CreateFeelers --------------------------
        //
        //  Creates the antenna utilized by WallAvoidance
        //------------------------------------------------------------------------
        void CreateFeelers()
        {
            //feeler pointing straight in front
            _feelers[0] = _bot.Pos() + _wallDetectionFeelerLength *
                _bot.Heading() * _bot.Speed();

            //feeler to left
            Vector2 temp = _bot.Heading();
            Transformations.Vec2DRotateAroundOrigin(ref temp, (Fix) (Math.PI / 2 * 3.5));
            _feelers[1] = _bot.Pos() + _wallDetectionFeelerLength / 2 * temp;

            //feeler to right
            temp = _bot.Heading();
            Transformations.Vec2DRotateAroundOrigin(ref temp, (Fix) (Math.PI / 2 * 0.5));
            _feelers[2] = _bot.Pos() + _wallDetectionFeelerLength / 2 * temp;
        }

        //--------------------------- WallAvoidance --------------------------------
        //
        //  This returns a steering force that will keep the agent away from any
        //  walls it may encounter
        //------------------------------------------------------------------------
        Vector2 WallAvoidance(List<Wall2D> walls)
        {
            //the feelers are contained in a std::vector, _feelers
            CreateFeelers();

            Fix DistToThisIP = Fix.Zero;
            Fix DistToClosestIP = Fix.MaxValue;

            //this will hold an index into the vector of walls
            int ClosestWall = -1;

            Vector2 SteeringForce = Vector2.zero;
            Vector2 point = Vector2.zero; //used for storing temporary info
            Vector2 ClosestPoint = Vector2.zero;

            //examine each feeler in turn
            for (int flr = 0; flr < _feelers.Length; ++flr)
            {
                //run through each wall checking for any intersection points
                for (int w = 0; w < walls.Count; ++w)
                {
                    if (Geometry.LineIntersection2D(_bot.Pos(),
                        _feelers[flr],
                        walls[w].From(),
                        walls[w].To(),
                        ref DistToThisIP,
                        ref point))
                    {
                        //is this the closest found so far? If so keep a record
                        if (DistToThisIP < DistToClosestIP)
                        {
                            DistToClosestIP = DistToThisIP;

                            ClosestWall = w;

                            ClosestPoint = point;
                        }
                    }
                } //next wall


                //if an intersection point has been detected, calculate a force  
                //that will direct the agent away
                if (ClosestWall >= 0)
                {
                    //calculate by what distance the projected position of the agent
                    //will overshoot the wall
                    Vector2 OverShoot = _feelers[flr] - ClosestPoint;

                    //create a force in the direction of the wall normal, with a 
                    //magnitude of the overshoot
                    SteeringForce = walls[ClosestWall].Normal() * OverShoot.magnitude;
                }
            } //next feeler

            return SteeringForce;
        }

        //---------------------------- Separation --------------------------------
        //
        // this calculates a force repelling from the other neighbors
        //------------------------------------------------------------------------
        Vector2 Separation(List<RavenBot> neighbors)
        {
            //iterate through all the neighbors and calculate the vector from the
            Vector2 SteeringForce = Vector2.zero;
            foreach (var it in neighbors)
            {
                //make sure this agent isn't included in the calculations and that
                //the agent being examined is close enough. ***also make sure it doesn't
                //include the evade target ***
                if ((it != _bot) && it.IsTagged() &&
                    (it != _targetAgent1))
                {
                    Vector2 ToAgent = _bot.Pos() - it.Pos();

                    //scale the force inversely proportional to the agents distance  
                    //from its neighbor.
                    SteeringForce += ToAgent.normalized / ToAgent.magnitude;
                }
            }


            return SteeringForce;
        }
    }
}